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Abstract: In this paper, a novel hybrid sensing method for tracking an ultrasonic 
endoscope within the gastrointestinal (GI) track is presented, and the prototype of the 
tracking system is also developed. We implement 6-DOF localization by sensing 
integration and information fusion. On the hardware level, a tri-axis gyroscope and 
accelerometer, and a magnetic angular rate and gravity (MARG) sensor array are attached 
at the end of endoscopes, and three symmetric cylindrical coils are placed around patients' 
abdomens. On the algorithm level, an adaptive fast quaternion convergence (AFQC) 
algorithm is introduced to determine the orientation by fusing inertial/magnetic 
measurements, in which the effects of magnetic disturbance and acceleration are estimated 
to gain an adaptive convergence output. A simplified electro-magnetic tracking (SEMT) 
algorithm for dimensional position is also implemented, which can easily integrate the 
AFQC's results and magnetic measurements. Subsequently, the average position error is 
under 0.3 cm by reasonable setting, and the average orientation error is 1° without noise. If 
magnetic disturbance or acceleration exists, the average orientation error can be controlled to 
less than 3.5°. 
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1. Introduction 

Ultrasonic endoscopes are widely used in examining organs adjacent to the gastrointestinal (GI) 
track [1-4]. However, with the restrictions of ultrasonic images, identification of anatomical structures 
is difficult [5]. To assist endoscopic surgeries, navigation systems are desirable. Electro-magnetic 
tracking (EMT) method has become a common practice in tracking endoscopes, including flexible [5-7] 
and capsule ones [8-10]. 

In existing navigation systems for flexible endoscopes, magnetic generators are placed externally, 
while sensors are attached to the devices. The generators produce specific magnetic fields, measured 
by the sensors. In a clean environment, magnetic measurements are relatively stable; but when metal 
objects are near the system, distortions will appear and may significantly affect the measurement 
results. For instance, Ren [11] has reported that a small metal object can cause 10 degrees of 
distortions in a NDI (NDI, Waterloo, ON, Canada) EMT device. For a tracking system for ultrasonic 
endoscopes, the scan head is the major interference source, which will continuously disturb estimates [12]. 
Equipment like metallic loops, wire guides, and catheters will also influence the outputs. Furthermore, 
with respect to basic principles, the traditional EMT methods are generally established on approximate 
models, assuming magnets are dipoles [13,14] or coils are single-term [15]. The processes would 
undoubtedly introduce measuring errors. 

To overcome EMT's sensibility to magnetic distortion, hybrid sensing technologies that fuse EMT 
with other tracking methods are promising [16]. Commonly, an optical tracking [17,18] or inertial 
navigation system (INS) [19,20] can be adopted to assist EMT. However, restricted by the line-of-sight 
conditions [21], integrating EMT and INS becomes the most feasible solution for us, so in our 
solution, an INS is applied. This subsystem consists of a tri-axis gyroscope and accelerometer, and a 
magnetic angular rate and gravity (MARG) sensor array. Though the gyroscope can estimate 
orientations in dynamic motions, its measuring errors grow unbounded because of the bias [22]. At the 
same time, the gyro-free part — containing the other two sensors — is able to provide drift-free 
estimation, which can be smoothed by the gyroscope's signals. In the gyro-free part, gravitational 
acceleration and geomagnetic field are chosen to measure the orientation, where common strategies 
include TRIAD, QUEST [23], QFA [24] and Gauss-Newton (G-N) [25], and they can also be fused 
with the outputs of the gyroscope to obtain optimal results through a Kalman filter [26,27]. In all these 
methods, the sensor data collected by the accelerometer and the magnetometer are regarded as values 
of gravity and geomagnetic field. Sabatini [28] figured out that there are limitations in such operations, 
because the outputs of the accelerometer also include accelerated velocities, which are hard to 
separate, and the outputs of the magnetometer may be distorted by ferromagnetic materials. 

To restrain interferences, noises should be considered in estimates. Roetenberg [29] has 
implemented modeling of the patterns of noises to reduce their influences through a Kalman filter. 
However, two difficulties restrict application of the method to endoscopic localization. Firstly, the 
disturbances of sensors are assumed as first-order Markov processes that are driven by white noises. 
For changeable environments containing multiple noises, the assumptions are not enough to quantify 
influences of disturbances on orientation estimation. In an operating room, the disturbing sources are 
various, ranging from metal instruments to electronic equipment, so it is hard to characterize all 
magnetic disturbances through one uniform model. Secondly, because multiple Markov models are 
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introduced in the filter, the algorithm's complexity is relatively high, which may limit real-time 
applications. When tracking an endoscope, continuous calculation over a relatively long time is needed, so 
the speed of the method is also a critical factor. In a word, a more efficient algorithm with anti-jamming 
capability is needed. 

For position determination, integrating acceleration signals [30] — the most common practice in 
human motion analysis based on INS — is impractical in tracking endoscopes. That is because speed of 
endoscopes is relative slow when moving in vivo, which makes the accelerometer's signals easy to be 
masked by its noises. Therefore this paper still applies the traditional method in the EMT 
subsystem — measuring the generators' fields through the magnetometer — for tracking real-time 
position. Different from typical EMT methods, the results of orientation estimates from the MARG 
subsystem are employed to reduce dependences on magnetic sensing further and simplify the solving 
process for position measurements. 

The organization of this paper is as follows: Section 2 the gives architecture of our navigation 
system. In Sections 3 and 4, we present the adaptive fast quaternion convergence (AFQC) and 
simplified electro-magnetic tracking (SEMT) algorithms. In Section 5, we propose the experimental 
and evaluation methods. In Section 6, we present and discuss the experimental results related to the 
localization accuracy, in which anti-interference ability is tested. Finally, conclusions are given. 

2. System Architecture 

Our overall scheme is shown in Figure 1. The system can be divided into four parts. Part 1 is the 
MARG sensor, which consists of an InvenSense MPU6050 (InvenSense, San Jose, CA, USA) 16 bit 
resolution inertial measurement unit (containing a gyroscope and an accelerometer) and a 16 bit 
resolution Honeywell HMC5883L magnetometer (Honeywell, Morristown, NJ, USA). With suitable 
printed circuit board (PCB) design, the sensor is minimized to ensure that it can be sealed in a glass 
tube with a diameter of 8 mm, which is shown in Figure 2. Part 2 is the coil array, including three 
symmetric cylindrical coils. The thickness of coils is 1 cm and the radius is 8 cm. With proper 
arrangement, they can suitably encircle patients' abdomens. Part 3 is the control system, containing a 
C8051F340 MCU and a PC. The MCU is responsible for collecting and uploading raw sensor data 
with a frequency of 20 Hz, as well as exciting the coils under the control of the computer. Part 4 is 
display unit used to display our application's interference that is shown in Figure 3 where the 
ultrasound scan section is demonstrated by a highlighted sector. 

A typical tracking cycle consists of four sampling cycles. An arbitrary one is demonstrated in 
Figure 4, in which four samples are indicated as sample 0, 1, 2 and 3. In sample 0, the sensor array 
outputs polluted gravitational acceleration, polluted geomagnetic flux density and polluted angular 
velocity. The initial estimate of orientation is gained by integrating the gyroscope's signals. Then it is 
converged by the outputs of the accelerator and the magnetometer, where AFQC is designed to add 
adaptive disposal for reducing influences of the noises. On sample / (/ = 1, 2, 3), the magnetometer 
measures the sums of Coil / and geomagnetic flux density, meanwhile the orientation is continually 
updated by the gyroscope. By synthesizing the information of samples from 1 to 3 and the result of 
AFQC, we can calculate the real-time position through SEMT. 
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Figure 1. Overview of the ultrasonic endoscope tracking system. 




Figure 2. The minimized MARG sensor array, (a) The 3D model of the senor array's PCB; 
(b) The photo of the senor array which is sealed in a glass tube. 




(a) (b) 
Figure 3. The interface of the ultrasonic endoscope tracking system. 
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To achieve the work process above, we should strictly control the operating sequences of the three 
coils. Firstly, we add a latency time (latency time 1) between sample 0 and the beginning of charging 
Coil 1 to avoid possible disturbances. After that, a constant direct current (DC) voltage is applied on 
Coil 1. Because of hysteresis effects, there will be a delay (delay 1) before the magnetic intensity 
become constant. Then a sampling window with flat singles is available until Coil 1 begins to 
discharge. For control convenience, sample 1 is set at the absolute middle of sampling window. For the 
same reason, latency time 1 is also added between sample 1 and the beginning of charging Coil 2. 
Finally, after Coil 1 loses its magnetism completely and the added latency 2 — which can avoid Coil l's 
influences on later samples — sample 2 arrives. Because all three coils share the same electric and 
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mechanical design, their control process has symmetry, and the sequences between sample 2 and the 
next tracking cycle are similar. 

Figure 4. The working sequence of the ultrasonic endoscope tracking system. 
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3. Orientation Algorithm 



3.1. Nomenclature 



0' - X'Y'Z': The body frame 
0 — XYZ: The earth frame 
x n : Vector in the earth frame 
: Vector in the body frame 
x: Measured vector with noises 
C^: Rotation matrix from frame m to frame n 
x: Estimation 



3.2. Sensor Model 

Gyroscope: the gyroscope's output can be described as the resultant of the angular velocity o>, the 
bias b and the white noise w^: 

a> = a> + b + (1) 

Accelerometer. the accelerometer's output can be described as the resultant of the gravity g and the 
linear acceleration a and the white noise w^: 

g = -g + a + (2) 

Magnetometer, the magnetometers' output can be described as follow, where C 5/ is the soft iron 
error matrix, and m HI is the hard iron error, and w m is the white noise: 

in = C 5/ (m + m^) + (3) 



Sensors 2014, 14 



9966 



3.3. Quaternion Model 

Orientation is often defined as a series of rotations from the Earth frame to the body frame. 
Quaternion is a common method to indicate such rotations, which effectively avoid the problem of 
singularity in the Euler angle. In this paper, quaternion is expressed as follow: 



Q = 



qi 
q2 
Lq 3 



(4) 



The rotation matrix from the Earth frame to the body frame can be described in quaternion terms: 

lo + qi-ql-ql 2 (q 0 q 3 -i-q^) 2^3 -q 0 q 2 ) ' 
2(qiq 2 + q3qo) qo-qi + qi-ql 2^! + q 2 q 3 ) 

2(q 0 q2 + qiq 3 ) 2(q 2 q 3 -q 0 q 1 ) q 2 0 - qf - q 2 2 + q|j 



C£(Q) = 



(5) 



The orientation of a rigid body (e.g., the scan head of an ultrasonic endoscope) is updated through a 
quaternion kinetic differential Equation: 



Q = -Q®to = n Q 



1 

fl = 2 



0 — CO x —<^>y — 0) z 



0), 



0 

-0) z 
0), 



0 

—00. 



-00j 



(6) 



(7) 



J y ~w x 0 

where 0) x , 0) y and 0) z are tri-axial components of the angular velocity. 

According to the differential equation, the recurrence formula of quaternion in discrete sampling is 
deduced easily as Equation (8), in which fl t is comprised of the angular velocity of the moment of t, At 
is the sampling time: 

Qt+At = e %At Q t (8) 

3.4. AFQC Structure 

In the AFQC algorithm, the estimations — updated by the gyroscope — are converged by the 
accelerometer and magnetometer signals. Self-adaptive weight factors are designed to judge 
discrepancies between sensor signals and true values of the gravity and geomagnetic field. 

We introduce an estimation matrix , which is structured by quaternion as Equation (5). 
Traditionally, to observe the estimation error, the minimum-squared-error criterion is used. The error 
function E is defined as [25]: 



F = 



g b ~ C b ng n 



m" - C*m". 
E = F T ■ F 



(9) 
(10) 



where: 



g^: is a 3 x 1 vector of the accelerometer output in the body frame. 
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m b : is a 3 x 1 vector of the magnetometer output in the body frame. 

g n : is a 3 X 1 vector of the true gravity value in the Earth frame. 

m n : is a 3 x 1 vector of the true geomagnetic value in the Earth frame. 

By minimizing the error, the orientation estimation will be converted. However, according to the 
sensor model above, the accelerometer and the magnetometer don't strictly output the true gravity 
value and the true geomagnetic value, which will decrease the accuracy or even invalidate the 
convergence. To solve such problems, we build an adaptive error function E ad ; 



E ac* - 



- II m 7 



Irrv 



+ 



g' 



g 71 



(m* - c£m^) (11) 



In the corrected error function, 



distinguishes the difference between magnetometer 
signals and true geomagnetic values by comparing their relative magnitudes, which functions as the 
judgment of the magnetometer's reliability in AFQC. When magnetic disturbances exist, 



increases the weight of (g b — C^g n ) . As a result, the accelerometer signals will be dominant in the 

convergence process, and the role of the magnetometer output will be weakened. Similarly, when the 
endoscope is accelerating, the outputs of the magnetometer will dominate the convergence, which can 
effectively reduce the impact of linear acceleration to attitude algorithm. 

On the basis of the adaptive error function, a new objective function can be easily built as: 




(g b - c£g n ) 



(12) 



G-N method can minimize the error conveniently, which is expressed as: 

Qr 1 = Q2 - a< Dla (Qk)Jad (Qfc)] -1 J L (Qfc)Fab (Q£)> 



Jad(Q2) = 



dQ n k 



(13) 
(14) 



where Q£ is the result of the nth G-N convergence in the kth tracking cycle. 

Because nonlinear optimizations are very sensitive to initial values, if the selection is undeserved, 
only local minima or diverged results will be obtained. In the kth tracking cycle, after convergence, the 
estimation of the orientation is Q^. At the moment of k ■ 4At + iAt (/ = 0, 1,2, 3), the fl matrices 
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constructed by the corresponding angular velocities are il fc . 4 At+iAt- According to Equation (8), the 
initial value Q^i °f the £+lth tracking cycle follows as: 

In a short period of time, the cumulative error of gyroscope caused by the bias is not obvious, so 
that the initial value of the k + 1th cycle is close enough to the true value, which can ensure the 
effectiveness of the convergence. 

4. Position Algorithm 

4.1. Modeling of Circular Coil 

For a single turn circular coil — whose radius is a and carrying current is I — and an arbitrary point 
P, the orthogonal coordinate system O c — X C Y C Z C is built, which is shown in Figure 5. The base 
vectors of three axes are e^, and e^. O c is located at the center of the coil. X c axis and Y c axis are 

along radial directions of the coil. Z c axis is perpendicular to the coil. P is at the plane X c O c Z c and its 
coordinate is (x, 0, z). 

Figure 5. Analyzing on the magnetic field of a single turn circular coil. 




We choose a point 5 on the coil, whose coordinates are (acoscf>, asincf>, 0). Considering that the 
permeability of the human body is very close to that of air, the magnetic field of the current element 
Idl which located at 5 can be calculated by the Biot-Savart law as follows: 

u o Idl x r 



dB = 



4 31 



(16) 



r = (x — acoscj)) ■ — asincf> ■ + z ■ (17) 

dl = a(— sincj) ■ + coscf) ■ e^)dcf> (18) 

where r is the vector from 5 to P, and /i 0 i s the permeability of air. 

Integrating along the closed loop, the magnetic flux density at P exited by the coil is as follows: 

|i 0 Ia f 2n zcos <$> ■ + zsin 4> ■ + (a — xcoscj)) ■ 



B = 



4tt 



■dc|> 



(z 2 + a 2 + x 2 — 2axcoscf>)2 
In which the component along the Y c axis can be expressed by: 



(19) 
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B y = 



|i 0 Ia r zsincj) 

= ^ 3 d * = 0 (20) 

J ° (z 2 + a 2 + x 2 - 2axcosc|))2 

From above, we can conclude that for any point in space, the magnetic flux density excited by a 
single turn circular coil is parallel to one specific plane, which is through the selected point and the 
center of the coil, and perpendicular to the coil. We define this plane as the target plane. 

A symmetric cylindrical coil consists of many single turned circular coils. These single turn coils are 
parallel to each other and share one midperpendicular. Therefore target planes of the single turn 
circular coils strictly overlap, and any of these planes can be defined as the target plane of the 
cylindrical coil. 

4.2. SEMT Structure 

We apply the general conclusions above to our localization system. In the Earth frame, the 
coordinates of the geometric centers of three coils are ( x ;> Yi> z ;) (i = 1,2,3), and the cross sections 
through the coils' centers are as follows: 

A 4 (x - x,) + B,(y - y 4 ) + Q(z - z 4 ) = 0 (i = 1,2,3) (21) 

where (x, y, z) is an arbitrary point on the cross sections, and (A t , B if Q) is one of the cross 
sections' normal vectors. 

In the Ml positioning cycle, after convergence, the estimation of orientation through AFQC is Q^. 
At the moment of k ■ 4At + i A t, fl Matrix obtained by the gyroscope is Cl k . 4 A t +iAt respectively, and 
the rotation matrix from the body frame to the Earth frame is expressed as follows according to 
Equation (8): 

C n k -4At+iAt = e S r=o^/c;A t+At -At ^ ( - = ^ (22) 



In the body frame, the output of the magnetometer at the k ■ 4At + iAt moment is m c+G k .4 At+ i At -> 
which is the sum of magnetic flux density of Coil i and the Earth in the body frame. Transforming it to 
the Earth frame we can calculate Coil f s magnetic flux density by Equation (23): 

™l k . 4At+iAt = (C b n k . 4M+iAt ) T ■ <To k . 4At+iAt - (i = 1,2,3) (23) 

According to the definition of target planes, the normal vector of Coil i's target plane at the 
k ■ 4At + iAt moment is both perpendicular to its magnetic flux density and normal vector, so the 
normal vector of this plane can be calculated as follows: 

(A*k-4At+iAt> ^k-AAt+iAv ^k-AAt+iAt) = (Ao Q) x m c k .4 At +i At (i = 1>2,3) (24) 

Considering the target plane is through the center of Coil i, the target plane of Coil i is: 

Ak-4At+iAt( x ~ x i) + B k . 4At+iAt (y — yi) + Ck.4 A t+iAt( z ~ z i) = 0(i = 1,2,3) (25) 

The three target planes of three coils intersect at the point where the sensor located. Therefore the 
position of the sensor can be calculated by solving the simultaneous equations of the three target 
planes, expressed by Equation (25). 
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4.3. Accuracy Analysis for SEMT 



In real working space, sensors' inherent noises and magnetic distortions are unavoidable. 
These factors can disturb the determinations of target planes, and consequently decrease SEMT's 
accuracy. To guarantee good performance during tracking, SEMT's response to noises and distortions 
should be analyzed carefully. We still adopt the model of Figure 5 for illustrating SEMT's stability, 
where another points is chosen, as shown in Figure 6. 

Figure 6. Analyzing on errors of SEMT. 
D 




The magnetic flux density on A excited by the coil is B A , which can be expressed as 
(B Ax , B Ay , B Az ) . The target plane of A is AA'D'D. If the angle formed by 's horizontal 
component and X c axis — defined as 0 — is determined, the target plane is also solely determined. This 
angle 0 is expressed as: 

(26) 



0 = actan 



B 



Ax 



Exerting a small disturbance AB^ to B Ax , 0 changes to 0 + A0 accordingly. AQ/AB Ax can be 
introduced to determine the disturbing degree of B Ax to 0, which then determine the disturbing degree 
of B Ax to the target plane. If - — is larger, disturb of B Ax to the target plane is more evident. 

dB Ax 

Similarly, dd 



dB Ay 



can indicate the disturbing degree of B Ay to the target plane: 



30 



dB Ax 
50 




3B 



Ay 



= \n Ax + 



B 



Ay 



B 



(27) 



(28) 



Ax 



From Equations (27) and (28) we reach that when B Ax or B Ay is smaller; the degree of their 

disturbance of the determination of the target plane is larger. When the sensor is near the axis wire of 
the coil, B Ax and B Ay are both very small, the noises of the magnetometer will affect the determination 

of 0 in the greatest extent, which in turn disturbs the output of the target plane, and eventually 
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increases the positioning errors. Namely, the area near the coils' axes has the biggest error for position 
determination. As a result, we should exclude the axis wires of the coils in tracking area. 

In addition, if the three target planes meet at a straight line, the algorithm fails. Such a condition 
will form singular points for localization. Hence in the design of the coils' layout style, we should 
ensure that the singular points are also out of the tracking area. 

Apart from the coils' location, the sampling cycle can influence SEMT's accuracy too. 
Theoretically speaking, in the process of motivating three coils, the position of the sensor must be 
basically unchanged, and the gyroscope's orientation estimates should be adequately accurate. For an 
ultrasonic endoscope, which moves in the GI tract with low speed, as long as the sampling time At is 
small enough, the conditions are easy to implement. The confine of the sampling time is in conformity 
with the attitude tracking requirements. 

To sum up, two elements are essential for the SEMT's accuracy. The first is a reasonable layout 
style for the three coils, which means the axis wires and singular points should be out of the position 
area. The second is a short enough sampling cycle which is particularly desirable in dynamic tracking. 

4.4. Integrated 6-DOF Tracking 

From the above, a complete 6-DOF positioning scheme is concluded as illustrated in Figure 7. 
Through additional adaptive processing, G-N converge will achieve good anti-noise characters; 
and by synthesizing orientation estimates by INS and EMT technology, the positioning method is 
greatly simplified. 

Figure 7. Tracking solution for ultrasonic endoscopes. 
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5. Experimental Methods 

5.1. Experimental Equipment 

We designed an assessment device by simulating Hummel' s scheme [31] to evaluate our 
localization results. The device can provide standardized attitude angle values every 15° from 0° to 
360°, and standardized position values on concentric circles with radii of 2.5, 5, 7.5, and 10 cm every 
15°. The experimental positioning range was bigger than the abdominal cavity volume of adults. We 
also introduced a three-axis turntable to assess orientation estimation in dynamic state. Axes of the 
turntable were equated with potentiometers. The standard values of orientation can be obtained by A/D 
conversion (Figure 8). 

Figure 8. (a) The assessment device; (b) The three-axis turntable; (c) The experimental platform. 




(c) 

5.2. Experimental Procedure 

The complete experiment consisted of four parts. In the first experiment, we recorded 
root-mean-square error (RMSE) of AFQC with Euler angle in static and dynamic states, where a 
classical G-N method was added as a comparison group [25]. 

In the second experiment, the anti-interference ability of AFQC was tested in static tracking. Firstly 
a common laparoscopic instrument was put close to the sensor during tracking to apply magnetic 
interference, and its magnitude was recorded through the magnetometer's signals. Then the sensor was 
pushed to exert accelerated interference, whose magnitude was recorded by the accelerometer's 
signals. The static RMSE under interferences were acquired to quantify performances of AFQC. G-N 
convergence without adaptive complement acted as a comparison group too [25]. 
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In the third experiment, the positioning accuracy of SEMT was tested. We designed two kinds of 
coil layout styles to verify the relationship between noise responses of SEMT and the coils' display 
methods. For both styles, the outputs of SEMT were sampled 20 times at each point to obtain 
the RMSE. 

The first style was demonstrated as Figure 9. Thee coils were all perpendicular to the horizontal 
plane, and the Earth frame's origin was located at the geometric center of Coil2. From Coil 1 to Coil 3, 
their cross sections were expressed as Equations (29)— (31), respectively, which were all obtained 
in centimeters: 



1 V3 
--(x-17.5)+ — (y-8) = 0 

y = o 

1 a/3" 
-(x+17.5) + — (y-5) = 0 



(29) 
(30) 
(31) 



Accordingly, obverted points' coordinates on the assessment device were Equations (32) to (34): 

x = — r ■ sin (j ■ tt/6) (32) 
y = 17.5 - r ■ cos (j ■ tt/6) (33) 
z = -1.5 (r = 2.5,5,7-5,10; j = o,l, ... 11) (34) 

Figure 9. The first layout style of the experimental equipment (top view). 




The assessment 
device 



The second style was as demonstrated in Figure 10. Three coils were expressed as Equations (35)-(37), 
and measured points were Equations (38)-(40). All coordinates were obtained in centimeters too: 



V3 / 25 



r-\ If 25\ V3/ 12+V3\ 

V3)+^ + t) + t( z — H = 0 

V3/ 25 r-\ 1/ 25\ V3/ 
_( I+ _V3) + _( y+ _) + _^_ 



12 + V3^ 



- 2 iy-2V + -[z —^ = 0 

x = r ■ sin (j ■ tt/6) 



(35) 

(36) 

(37) 
(38) 
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y = r ■ cos (j ■ it/6) 



(39) 



z = 7 (r = 2.5,5,7.5,10; j = 0,1, - H) 



(40) 



In the fourth experiment, influences of the sampling cycle on the accuracy in dynamic tracking were 
tested. In the algorithm structure sections we have proven that the sampling cycle can affect the 
accuracy of both AFQC and SEMT. To measure specific relations between them, a group of contrast 
experiments were carried out, in which sampling cycles were set as 0.05, 0.06, 0.07. . .0.14 s. For every 
different cycle, results of roll angle and positional measurement on X axis were recorded, and the 
corresponding absolute errors were then worked out. 



6. Results and Discussion 

6.1. Orientation Measurement 

Table 1 demonstrates the RMSE of AFQC and traditional G-N algorithm on orientation estimation 
without disturbances. As can be seen from the table, the RMSE in static state of the two algorithms are 
essentially equal on three Euler angles. However in dynamic state, the performances of AFQC 
are better than G-N, which is caused by the inhibitory effect of AFQC on accelerations in the 
convergence process. 



Figure 10. The second layout style of the experimental equipment. 




z 



Y 



Table 1. The RMSE of AFQC and N-G without disturbances. 



Euler Parameter 



AFQC 



G-N 



RMSE (pitch angle) static 
RMSE (pitch angle) dynamic 



0.694° 
0.753° 
0.696° 
0.723° 
1.069° 
1.168° 



0.676° 
0.823° 
0.701° 
0.814° 
1.127° 
1.563° 



RMSE (roll angle) static 
RMSE (roll angle) dynamic 
RMSE (heading angle) static 



RMSE (heading angle) dynamic 
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In the second experiment, noises' magnitudes and relevant RMSE of AFQC and G-N were shown 
in Figures 1 1 and 12. No matter the acceleration or magnetic disturbances exerted, AFQC can basically 
control the noises and obtain stable outputs, while the non-adaptive algorithm fails. Specific RMSE of 
AFQC in the present of disturbances is recorded in Table 2. In such circumstances, total RMSE of 
AFQC is controlled under 3.5°. 



Figure 11. Typical results for estimation errors when magnetic disturbances applied, 
(a) Magnetic field magnitude. Magnetometer's signals become variable when magnetic 
field is disturbed; (b) RMSE of AFQC and G-N. 




2 3 
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2 3 4 

time (second) 
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Figure 12. Typical results for estimation errors when acceleration disturbances applied, 
(a) Magnitude of specific force. Acceleration's signals become variable when the sensor is 
moving; (b) Orientation outputs of AFQC and G-N. 





(a) 



(b) 



Our orientation estimation algorithm is verified in a wide variety of environments (static, dynamic, 
with or without disturbances). Without interferences, AFQC can output stable and effective results. 
Although acceleration or magnetic interferences exist, errors can be controlled to ensure accurate results. 

It is worth noting that in INS many algorithms are built on the convergence through double vectors 
without adaptive characteristics [25-27], which will eventually fail when applying noises. At the same 
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time, the uncertainty of magnetic noises also limits anti-interference algorithms attempting to build 
Markov models of sensor output pattern [29]. AFQC avoids pattern analysis which involves complex 
theoretical assumptions, and still ensures enough anti-jamming capability. Compared with EMT 
tracking methods reported by other groups [8-10], we have achieved higher accuracy in a clean 
environment. Disturbances are also considered in our method, which are ignored in the papers above. 
When disturbances exist, an accuracy of 3.5° is much higher than NDFs 10° reported by Ren [11]. 



Table 2. The RMSE of AFQC with disturbances. 



Euler Parameter 


Magnetic Disturbances 


Acceleration Disturbances 


RMSE (pitch angle) 


1.2205° 


1.5948° 


RMSE (roll angle) 


1.5608° 


1.5551° 


RMSE (heading angle) 


1.6877° 


2.1288° 


RMSE (in total) 


2.6027° 


3.0764° 



Essentially, AFQC judges the reliability of the acceleration and magnetometer by comparing the 
magnitudes of their outputs with theoretical values. However, gravitational acceleration and 
magnetic flux density are vectors; magnitudes cannot strictly characterize disturbances. Considering 
our experimental results, we still can safely conclude the adaptive processing is enough for 
endoscopic tracking. 

6.2. Position Measurement 

The tracking results for both two layout styles are shown in Figure 13. We can find the great 
differences between them. For the first layout style, distribution of SEMT's results is bimodal. 
On twenty-six points — which are defined as group 1 — measurements for the same spot are less 
dispersive between each other. Meanwhile on other twenty-two points (group 2), dispersions are much 
larger. For the second layout style, all forty-eight points share similar distributions which are almost 
the same as group 1 in the first style. 



Figure 13. The results of the positioning experiment, (a) Results from the first layout style; 
(b) Results from the second layout style. 




(a) (b) 
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To reveal their differences mathematically, we carry out RMSE on every point for both layout 
styles, and demonstrate them by the box plots shown in Figure 14. For the first style, we can find that 
group 1 is not only less dispersive than group 2, but also more focused on the real values. 
Experimental results from the second style are all well focused on the true values, making 
measurements much more stable and trustworthy. 

Apart from the box plot, specific average values and standard deviations for SEMT's errors in these 
two styles are also recorded in Tables 3 and 4. Summarizing from the two tables, the first layout style's 
error is about 0.7 cm, while the second style's is within 0.3 cm. 



Figure 14. Distribution of SEMT's errors, (a) Errors of the first layout style. Group 1 
consists of the twenty-six points of higher accuracy, and group 2 consists of the other 
twenty two with lower accuracy; (b) Errors of the second layout style. 





(a) 



(b) 



Table 3. The error distribution of SEMT in the first layout style. 



RMSE 


Group 1 


Group 2 


average value 


0.2599 cm 


0.6365 cm 


standard deviation 


0.0170 cm 


0.0433 cm 



Table 4. The error distribution of SEMT in the second layout style. 



RMSE 


Value 


average value 


0.2396 cm 


standard deviation 


0.0166 cm 



It is the relative position between the observed area and the axes of the coils that cause the 
differences of the two styles. When adopting the first style, coils' axes traverse the tracking area; and 
measurements around axes are badly influenced as we have already proved. These points constitute 
group 2, whose errors are up to 7 cm. Meanwhile, other points away from the axes still offer 
satisfactory precision, and they are classified into group 1. However, by setting the layout method 
more wisely, the axes of three coils can be absolutely excluded from our position region as the second 
style does. Although the position area is unchanged, the second style can improve tracking results 
markedly and be directly adopted for practical application. 

Compared with accuracy reported by other groups [32,33], the accuracy of 0.3 cm is a bit lower, but 
it is still small and within acceptable limits [34,35]. On the other hand, in contrast with their large array 
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which contains dozens of coils and specialized high-speed circuits, our system structure is much more 
simplified, and only requires a low-cost integrated sensor, regular coils and basic hardware. Besides, 
SEMT contains no complex iterations which are commonly applied by them, so bulk calculating costs 
are effectively avoided. In a word, SEMT can achieve an effective accuracy through much lower 
systematic complexity in position estimation. 

6.3. Influences of Sampling Cycle on Accuracy 

Results of the fourth experiment are demonstrated in Figure 15. From them, we find that the 
accuracy of orientation estimate remains almost unchanged when the sampling cycles become longer. 
Meanwhile, the accuracy of position determination decreases, and it is even more obvious when the 
cycle is longer than 0.1 1 s. 

Figure 15. Absolute error of tracking for different sampling cycles, (a) RMSE of AFQC on 
roll angle; (b) RMSE of SEMT on X axis. 
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For AFQC, orientation estimates based on the gyroscope only provide an initial value, which is then 
converged by the accelerometer and the magnetometer's signals. As long as the gyroscope's 
accumulative errors are small enough to guarantee the error function's local minimum — obtained by 
convergences — is the global minimum, errors caused by the gyroscope can be effectively controlled. 
From the experimental results, we find this condition is easy to achieve. 

However, SEMT's situation is much more different. When obtaining analytic forms of target 
planes in the Earth frame, real-time orientation is needed. Because of the external fields generated by 
the coils, we cannot obtain the orientation from double-vector convergences. Outputs of the gyroscope 
are the only sources for estimation, so its bias will restrict the accuracy. What's more, sensors' 
movement between coils' excitation can change the magnetometer's measuring point, which will 
impact resolving target planes, and finally damage SEMT. Therefore, when the length of sampling 
cycle is beyond a certain value (in our system, it is 0.11 s), it is sufficient to disturb determination of 
target planes and lower SEMT's overall accuracy. Despite the advantages of improving the accuracy, it 
is impractical to reduce the sampling cycle further. This is because data sampling and coils' control are 
highly coordinated as motioned in the System Architecture section, and the inherent hysteresis in the 
materials used limits the minimum value of the sampling cycle. As a result, a sampling cycle of 0.05 s 
is the optimal choice for our system. 
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6.4. Contributions Relative to Similar Research 

Similar works are mainly reported by Ren in [11,19,20]. In his research, Ren combines a custom 
EMT subsystem designed in [32,33] — which contains a generator array bearing 48 coils and a 
receiving array bearing three coils — with a typical MARG sensor. Normally, the MARG provides the 
orientation estimates. If the acceleration or magnetic distortion are too high, the EMT subsystem will 
replace MARG to determine orientation. Then, in combination with the orientation result, MARG 
derives the real-time position through the kinematic relationship between position, velocity and 
acceleration. Compared with his work, we believe we have achieved at least three improvements. 

Firstly, we propose a new orientation estimation algorithm with anti-interference ability to resist 
acceleration and magnetic distortion. In Ren's solution, as EMT subsystem is adopted to reduce the 
influences from disturbances, and it needs to be very robust. To achieve this requirement, its hardware 
structure is very complex, including a huge generator array. The estimation algorithm, which includes 
complex strategies like Bayesian filtering [32] or particle filtering [33], is also very costly. On the 
contrary, our solution is much easier to accomplish. Only a low-cost MARG sensor and a solution for a 
basic least squares problem are required. Although the processes in hardware and algorithm are both 
largely predigested, the accuracy we get is impressive, especially with interferences. 

Secondly, we design a novel fusion method for position tracking and handle its potential shortages 
in a more detailed manner. In Ren's papers, position results are essentially determined through the 
double integral of the accelerator's output. In theory, inherent noises in the accelerator would decrease 
the sensor's signal-to-noise ratio, and devaluate the positional measurements. Unfortunately, Ren 
neither analyzes this potential threat nor provides an effective method to avoid it. On the contrary, 
when integrating magnetic measurements with orientation obtained by MARG, we put forward some 
factors that might influence its accuracy; it includes layout styles for coils and sampling cycle lengths. 
Then we evaluate and verify these inferences through sets of experiments. Based on them, optimal 
choices are proposed. As a result, errors are further controlled. 

Thirdly, we greatly simplify the EMT-INS hybrid system which can solve the contradiction 
between a tracking system's complexity and clinical practicability [16]. In an orientation estimate, an 
easy Gauss-Newton method with adaptive weights is proved sufficient for reliable outputs with 
anti-interference ability. In position determination, a generator array with only three coils is 
needed, and the corresponding fusion algorithm only including three linear equations is also very 
convenient for resolving. 

6.5. Deficiencies and Future Work 

There remain some limits to our system. First of all, accuracy for positional tracking is relatively 
low. We suppose this is caused by the coil manufacturing technique. During tracking, only if every coil 
is cylindrically symmetrical around every radial direction, can the determination for a target plane be 
totally accurate. It is a condition that only can be approached but not reached, so the accuracy we 
obtain is bounded by it. Second, magnetic distortion is still not overcome completely, especially in 
positional tracking. Currently, we just optimize SEMT's stability through avoiding points which are 
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vulnerable to magnetic disturbances, so more active methods that can resist magnetic distortion are 
required in the process for better performances. 

Aiming at these deficiencies, we will firstly improve our process of manufacturing the generators in 
order to achieve higher accuracy for SEMT. At the same time, active anti-interference techniques 
should be researched intensively, where more advanced hardware and algorithm research would be 
both desirable. 

7. Conclusions 

In summary, this paper develops a novel EMT-INS hybrid tracking system for ultrasonic 
endoscopes, where integrated algorithm and hardware prototype are both covered. For orientation 
estimates, a low-cost MARG is introduced. By adding two adaptive weights that can adjust themselves 
automatically according to acceleration and magnetic distortion, we successfully provide the 
traditional Gauss-Newton method with new anti-inference ability. 

For position determination, EMT subsystems are highly simplified. An easy hardware design with 
only three coils has been achieved. Accordingly, a new hybrid strategy named SEM can effectively 
fuse sensing information from the two subsystems. To obtain more stable output, factors relative to 
SEMT's accuracy are also carefully studied. On the whole, by fusing two sensing systems, we partly 
solve the instability within traditional EMT system, and also reduce its complexity in both the 
hardware and algorithm aspects. 
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